/**
 * @file RRTStarPlanner.cpp
 * @brief RRT*算法路径规划器实现
 */

#include "planners/RRTStarPlanner.h"
#include <chrono>
#include <cmath>
#include <algorithm>

namespace PathPlanning
{
RRTStarPlanner::RRTStarPlanner(std::shared_ptr<Map> map) : PlannerBase(map), gen_(rd_()), dis_(0.0, 1.0)
{
    config_ = std::make_shared<RRTStarConfig>();
}

PlanningResult RRTStarPlanner::plan(const Point& start, const Point& goal)
{
    PlanningResult result;
    auto           startTime = std::chrono::high_resolution_clock::now();

    if (!validateStartGoal(start, goal))
    {
        result.message = "Invalid start or goal position";
        return result;
    }

    auto rrtStarConfig = std::static_pointer_cast<RRTStarConfig>(config_);

    // 初始化树
    root_        = std::make_shared<RRTStarNode>(start);
    root_->gCost = 0.0;
    nodes_.clear();
    nodes_.push_back(root_);

    std::shared_ptr<RRTStarNode> goalNode   = nullptr;
    int                          iterations = 0;

    while (iterations < rrtStarConfig->maxIterations)
    {
        iterations++;

        // 随机采样（有一定概率直接采样目标点）
        Point randomPoint;
        if (dis_(gen_) < rrtStarConfig->goalBias)
        {
            randomPoint = goal;
        }
        else
        {
            randomPoint = randomSample();
        }

        // 找到最近的节点
        auto nearestNode = findNearestNode(randomPoint);

        // 从最近节点向采样点扩展
        Point newPoint = steer(nearestNode->position, randomPoint, rrtStarConfig->stepSize);

        // 碰撞检测
        if (!isPathCollisionFree(nearestNode->position, newPoint))
        {
            continue;
        }

        // 寻找附近的节点
        auto nearNodes = findNearNodes(newPoint, rrtStarConfig->searchRadius);

        // 选择最优父节点
        auto bestParent = chooseBestParent(nearNodes, newPoint);
        if (!bestParent)
        {
            bestParent = nearestNode;
        }

        // 创建新节点
        auto newNode   = std::make_shared<RRTStarNode>(newPoint);
        newNode->gCost = bestParent->gCost + euclideanDistance(bestParent->position, newPoint);

        // 将新节点加入树
        bestParent->addChild(newNode);
        nodes_.push_back(newNode);

        // 重连接操作
        rewire(newNode, nearNodes);

        // 记录探索节点（用于可视化）
        result.exploredNodes.push_back(Pose(newPoint.x, newPoint.y, 0));

        // 检查是否到达目标
        if (isGoalReached(newPoint, goal))
        {
            goalNode = newNode;
            std::cout << "Goal reached at iteration " << iterations << std::endl;

            // 不一定立即退出，继续优化
            if (rrtStarConfig->usePathOptimization && iterations < rrtStarConfig->maxIterations / 2)
            {
                // 继续优化一段时间
                continue;
            }
            else
            {
                break;
            }
        }

        // 可视化回调
        if (vizCallback_ && iterations % 100 == 0)
        {
            result.iterations = iterations;
            if (goalNode)
            {
                result.path = reconstructPath(goalNode);
            }
            vizCallback_(result);
        }
    }

    auto endTime        = std::chrono::high_resolution_clock::now();
    result.planningTime = std::chrono::duration<double>(endTime - startTime).count();
    result.iterations   = iterations;

    if (goalNode)
    {
        result.success = true;
        result.path    = reconstructPath(goalNode);

        // 计算路径长度
        for (size_t i = 1; i < result.path.size(); ++i)
        {
            result.pathLength += euclideanDistance(result.path[i - 1].toPoint(), result.path[i].toPoint());
        }
        result.message = "Path found successfully";
    }
    else
    {
        result.message = "Path not found within iteration limit";
    }

    return result;
}

Point RRTStarPlanner::randomSample()
{
    auto rrtStarConfig = std::static_pointer_cast<RRTStarConfig>(config_);

    // 在地图范围内随机采样
    int maxX = map_->getWidth() * map_->getResolution();
    int maxY = map_->getHeight() * map_->getResolution();

    while (true)
    {
        double x = dis_(gen_) * maxX;
        double y = dis_(gen_) * maxY;

        Point sample(x, y);

        // 确保采样点在自由空间
        if (!map_->isObstacle(sample))
        {
            return sample;
        }
    }
}

std::shared_ptr<RRTStarNode> RRTStarPlanner::findNearestNode(const Point& target)
{
    std::shared_ptr<RRTStarNode> nearest     = nullptr;
    double                       minDistance = std::numeric_limits<double>::max();

    for (const auto& node : nodes_)
    {
        double distance = euclideanDistance(node->position, target);
        if (distance < minDistance)
        {
            minDistance = distance;
            nearest     = node;
        }
    }

    return nearest;
}

Point RRTStarPlanner::steer(const Point& from, const Point& to, double maxDistance)
{
    double distance = euclideanDistance(from, to);

    if (distance <= maxDistance)
    {
        return to;
    }
    else
    {
        // 计算方向向量
        double dx = to.x - from.x;
        double dy = to.y - from.y;

        // 单位向量
        double length = std::sqrt(dx * dx + dy * dy);
        dx /= length;
        dy /= length;

        // 扩展指定距离
        return Point(from.x + dx * maxDistance, from.y + dy * maxDistance);
    }
}

bool RRTStarPlanner::isPathCollisionFree(const Point& from, const Point& to)
{
    auto rrtStarConfig = std::static_pointer_cast<RRTStarConfig>(config_);

    // 沿着路径采样检查碰撞
    double distance   = euclideanDistance(from, to);
    int    numSamples = std::max(2, static_cast<int>(distance / rrtStarConfig->stepSize * 10));

    for (int i = 0; i <= numSamples; ++i)
    {
        double t = static_cast<double>(i) / numSamples;
        double x = from.x + t * (to.x - from.x);
        double y = from.y + t * (to.y - from.y);

        if (map_->isObstacle(x, y))
        {
            return false;
        }
    }

    return true;
}

std::vector<std::shared_ptr<RRTStarNode>> RRTStarPlanner::findNearNodes(const Point& point, double radius)
{
    std::vector<std::shared_ptr<RRTStarNode>> nearNodes;

    for (const auto& node : nodes_)
    {
        if (euclideanDistance(node->position, point) <= radius)
        {
            nearNodes.push_back(node);
        }
    }

    return nearNodes;
}

std::shared_ptr<RRTStarNode>
RRTStarPlanner::chooseBestParent(const std::vector<std::shared_ptr<RRTStarNode>>& nearNodes, const Point& newPoint)
{
    std::shared_ptr<RRTStarNode> bestParent = nullptr;
    double                       minCost    = std::numeric_limits<double>::max();

    for (const auto& node : nearNodes)
    {
        // 检查路径是否无碰撞
        if (!isPathCollisionFree(node->position, newPoint))
        {
            continue;
        }

        // 计算代价
        double cost = node->gCost + euclideanDistance(node->position, newPoint);

        if (cost < minCost)
        {
            minCost    = cost;
            bestParent = node;
        }
    }

    return bestParent;
}

void RRTStarPlanner::rewire(std::shared_ptr<RRTStarNode>                     newNode,
                            const std::vector<std::shared_ptr<RRTStarNode>>& nearNodes)
{
    for (const auto& node : nearNodes)
    {
        // 跳过自身和父节点
        if (node == newNode || node == newNode->parent)
        {
            continue;
        }

        // 检查从新节点到该节点的路径是否无碰撞
        if (!isPathCollisionFree(newNode->position, node->position))
        {
            continue;
        }

        // 计算新代价
        double newCost = newNode->gCost + euclideanDistance(newNode->position, node->position);

        // 如果新代价更小，重新连接
        if (newCost < node->gCost)
        {
            // 从原父节点中移除
            auto oldParent = std::static_pointer_cast<RRTStarNode>(node->parent);
            if (oldParent)
            {
                auto& children = oldParent->children;
                children.erase(std::remove(children.begin(), children.end(), node), children.end());
            }

            // 设置新父节点
            newNode->addChild(node);
            node->gCost = newCost;

            // 递归更新子节点的代价
            std::function<void(std::shared_ptr<RRTStarNode>)> updateChildrenCost;
            updateChildrenCost = [&](std::shared_ptr<RRTStarNode> n) {
                for (auto child : n->children)
                {
                    child->gCost = n->gCost + euclideanDistance(n->position, child->position);
                    updateChildrenCost(child);
                }
            };

            updateChildrenCost(node);
        }
    }
}

bool RRTStarPlanner::isGoalReached(const Point& point, const Point& goal)
{
    auto rrtStarConfig = std::static_pointer_cast<RRTStarConfig>(config_);

    double distance = euclideanDistance(point, goal);
    return distance <= rrtStarConfig->optimizationTolerance;
}

std::vector<Pose> RRTStarPlanner::reconstructPath(std::shared_ptr<RRTStarNode> goalNode)
{
    std::vector<Pose> path;
    auto              currentNode = goalNode;

    while (currentNode != nullptr)
    {
        path.push_back(Pose(currentNode->position.x, currentNode->position.y, 0));
        if (currentNode->parent)
        {
            currentNode = std::static_pointer_cast<RRTStarNode>(currentNode->parent);
        }
        else
        {
            break;
        }
    }

    std::reverse(path.begin(), path.end());

    auto rrtStarConfig = std::static_pointer_cast<RRTStarConfig>(config_);
    if (rrtStarConfig->usePathOptimization)
    {
        return optimizePath(path);
    }

    return path;
}

std::vector<Pose> RRTStarPlanner::optimizePath(const std::vector<Pose>& path)
{
    if (path.size() <= 2)
    {
        return path;
    }

    std::vector<Pose> optimizedPath;
    optimizedPath.push_back(path[0]);

    size_t currentIndex = 0;

    while (currentIndex < path.size() - 1)
    {
        size_t nextIndex = path.size() - 1;

        // 从最远点开始检查，找到可以直接连接的最远点
        for (size_t i = path.size() - 1; i > currentIndex; --i)
        {
            Point from = path[currentIndex].toPoint();
            Point to   = path[i].toPoint();

            if (isPathCollisionFree(from, to))
            {
                nextIndex = i;
                break;
            }
        }

        optimizedPath.push_back(path[nextIndex]);
        currentIndex = nextIndex;
    }

    return optimizedPath;
}

} // namespace PathPlanning